#include "KinematicSensorPlot.h"

using namespace MMM;

KinematicSensorPlot::KinematicSensorPlot(KinematicSensorPtr sensor) :
    sensor(sensor)
{
}

std::vector<std::string> KinematicSensorPlot::getNames() {
    return sensor->getJointNames();
}

std::tuple<QVector<double>, QVector<double> > KinematicSensorPlot::getPlot(std::string name) {
    int index = -1;
    for (unsigned int i = 0; i < sensor->getJointNames().size(); i++) {
        if (sensor->getJointNames()[i] == name) {
            index = i;
            break;
        }
    }
    if (index < 0) throw Exception::MMMException("Plot for " + name + " not found.");

    QVector<double> x(sensor->getTimesteps().size()), y(sensor->getTimesteps().size());
    for (unsigned int j = 0; j < sensor->getTimesteps().size(); j++) {
        float timestep = sensor->getTimesteps()[j];
        MMM::KinematicSensorMeasurementPtr measurement = sensor->getDerivedMeasurement(timestep);
        x[j] = timestep;
        y[j] = measurement->getJointAngles()[index];
    }
    return std::make_tuple(x, y);
}

std::string KinematicSensorPlot::getSensorName() {
    return sensor->getUniqueName();
}
